﻿#include "linear_math.h"

#ifdef NDEBUG
#define assert(expr) ((void)0)
#else
#define assert(expr) ((expr) ? (void)0 : __debugbreak())
#endif

#define SIGN(a) ((a) >= 0 ? 1 : -1)
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) > (b) ? (b) : (a))

#define ACCURACY 1e-12
#define HALF_ACCURACY 1e-6
#define ITERATION 1000

// 初始化构造函数
CAGD::Vector::Vector(int D, double t) : m_dim(D)
{
    assert(D > 0);
    m_value = new double[D];
    for (int i = 0; i < D; m_value[i++] = t)
        ;
}

// 可以通过列表初始化
CAGD::Vector::Vector(std::initializer_list<double> l) : m_dim(l.size())
{
    m_value = new double[m_dim];
    auto j = l.begin();
    for (int d = 0; d < m_dim; ++d)
        m_value[d] = *j++;
}

// 拷贝构造
CAGD::Vector::Vector(const Vector &rhs) : m_dim(rhs.Size())
{
    m_value = new double[m_dim];
    for (int d = 0; d < m_dim; d++)
        m_value[d] = rhs.m_value[d];
}

// 获得向量长度
int CAGD::Vector::Size() const
{
    return m_dim;
}

// 返回引用
double &CAGD::Vector::operator[](int i)
{
    assert(i < m_dim && i >= 0);
    return m_value[i];
}

// 返回值
double CAGD::Vector::operator[](int i) const
{
    assert(i < m_dim && i >= 0);
    return m_value[i];
}

// 向量加减
CAGD::Vector CAGD::Vector::operator+(const Vector &rhs) const
{
    Vector res = *this;
    for (int i = 0; i < m_dim; i++)
        res[i] += rhs[i];
    return res;
}

CAGD::Vector CAGD::Vector::operator-(const Vector &rhs) const
{
    Vector res = *this;
    for (int i = 0; i < m_dim; i++)
        res[i] -= rhs[i];
    return res;
}

double CAGD::Vector::operator*(const Vector &rhs) const
{
    double res = 0;
    for (int i = 0; i < m_dim; i++)
        res += m_value[i] * rhs[i];
    return res;
}

// 常数乘除法
CAGD::Vector CAGD::Vector::operator*(double t) const
{
    Vector res = *this;
    for (int d = 0; d < m_dim; ++d)
        res[d] *= t;
    return res;
}

CAGD::Vector CAGD::Vector::operator/(double t) const
{
    assert(fabs(t) > FLT_MIN);

    Vector res = *this;
    for (int d = 0; d < m_dim; ++d)
        res[d] /= t;
    return res;
}

// 向量乘矩阵
CAGD::Matrix CAGD::Vector::operator*(const Matrix &rhs) const
{
    assert(rhs.Row() == 1);

    CAGD::Matrix res(m_dim, rhs.Col());
    for (int i = 0; i < res.Row(); ++i)
        for (int j = 0; j < res.Col(); ++j)
            res(i, j) = m_value[i] * rhs(0, j);
    return res;
}

// 赋值
void CAGD::Vector::operator=(const Vector &rhs)
{
    assert(m_dim == rhs.m_dim);

    for (int d = 0; d < m_dim; d++)
        m_value[d] = rhs[d];
}

// 取反操作
CAGD::Vector CAGD::Vector::operator-() const
{
    Vector res = *this;
    for (int d = 0; d < m_dim; ++d)
        res[d] *= -1;
    return res;
}

// 比较
bool CAGD::Vector::operator==(const Vector &rhs) const
{
    if (rhs.m_dim != m_dim)
        return false;

    for (int d = 0; d < m_dim; ++d)
        if (fabs(m_value[d] - rhs[d]) > HALF_ACCURACY)
            return false;
    return true;
}

// 冒泡排序
void CAGD::Vector::Sort()
{
    for (int i = 0; i < m_dim; i++)
    {
        for (int j = 1; j < m_dim; j++)
        {
            // 比较相邻的元素，如果后者小于前者就交换
            if (m_value[j] < m_value[j - 1])
            {
                double tmp = m_value[j];
                m_value[j] = m_value[j - 1];
                m_value[j - 1] = tmp;
            }
        }
    }
}

// 元素求和
double CAGD::Vector::Sum() const
{
    double a = 0;
    for (int i = 0; i < m_dim; i++)
        a += m_value[i];
    return a;
}

// 取绝对值
CAGD::Vector CAGD::Vector::Abs() const
{
    Vector res(m_dim);
    for (int d = 0; d < m_dim; ++d)
        res[d] = fabs(m_value[d]);
    return res;
}

// 获取符号向量
CAGD::Vector CAGD::Vector::Sgn() const
{
    Vector res(m_dim);
    for (int d = 0; d < m_dim; ++d)
        res[d] = (m_value[d] > 0 ? 1 : (m_value[d] < 0 ? -1 : 0));
    return res;
}

// 计算范数
double CAGD::Vector::Norm(int nt) const
{
    if (nt == 2)
        return sqrt((*this) * (*this));
    else if (nt == 1)
    {
        double a = 0;
        for (int i = 0; i < m_dim; ++i)
            a += fabs(m_value[i]);
        return a;
    }
    // 无穷范数
    else if (nt == 0)
    {
        double a = fabs(m_value[0]);
        for (int i = 1; i < m_dim; i++)
            a = MAX(fabs(m_value[i]), a);
        return a;
    }
    return 0;
}

// 取最小元
double CAGD::Vector::Min() const
{
    double a = m_value[0];
    for (int d = 1; d < m_dim; d++)
        if (m_value[d] < a)
            a = m_value[d];
    return a;
}

// 取最大元
double CAGD::Vector::Max() const
{
    double a = m_value[0];
    for (int d = 1; d < m_dim; d++)
        if (m_value[d] > a)
            a = m_value[d];
    return a;
}

// 取最大模的分量
int CAGD::Vector::MaxId() const
{
    int index = 0;
    for (int d = 1; d < m_dim; d++)
        index = (fabs(m_value[d]) > fabs(m_value[index]) ? d : index);
    return index;
}

// 转化成矩阵
CAGD::Matrix CAGD::Vector::ToBMatrix() const
{
    Matrix m(m_dim, 1);
    for (int i = 0; i < m_dim; i++)
        m(i, 0) = m_value[i];
    return m;
}

// 转置
CAGD::Matrix CAGD::Vector::Transpose() const
{
    Matrix m(1, m_dim);
    for (int i = 0; i < m_dim; i++)
        m(0, i) = m_value[i];
    return m;
}

// Householder 变换
double CAGD::Vector::HouseHolder(Vector &tx, Vector &v)
{
    // 使用无穷范数规范化防止溢出
    double eta = tx.Norm(0);
    Vector x = tx / eta;
    v = x;
    v[0] = 0;
    double sigma = x * x - x[0] * x[0];

    double beta = 0;
    if (fabs(sigma) > ACCURACY)
    {
        double alpha = sqrt(x[0] * x[0] + sigma);
        if (x[0] <= 0)
            v[0] = x[0] - alpha;
        // 如果是正的分量，则用特殊算法减小舍入误差
        else
            v[0] = -sigma / (x[0] + alpha);

        // 对 beta 乘 v[0] * v[0] ，抵消规格化为 1 的影响
        beta = 2 * v[0] * v[0] / (sigma + v[0] * v[0]);
        v = v / v[0];
    }
    return beta;
}

// Givens 变换
CAGD::Vector CAGD::Vector::Givens(Vector &v, int i, int j)
{
    double sqrtX = sqrt(v[i] * v[i] + v[j] * v[j]);
    if (sqrtX == 0)
        return {1, 0};
    return {v[i] / sqrtX, v[j] / sqrtX};
}

// 释放资源
CAGD::Vector::~Vector()
{
    delete[] m_value;
}

// 初始化构造函数
CAGD::Matrix::Matrix(int Row, int Col, double t) : m_row(Row), m_col(Col)
{
    assert(Row > 0 && Col > 0);

    m_value = new double *[Row];
    for (int i = 0; i < Row; i++)
    {
        m_value[i] = new double[Col];
        for (int j = 0; j < Col; j++)
            m_value[i][j] = t;
    }
}

// 通过列表初始化
CAGD::Matrix::Matrix(int Row, int Col, std::initializer_list<double> l) : m_row(Row), m_col(Col)
{
    assert(l.size() == Row * Col);

    m_value = new double *[Row];
    auto it = l.begin();
    for (int i = 0; i < Row; i++)
    {
        m_value[i] = new double[Col];
        for (int j = 0; j < Col; j++)
        {
            m_value[i][j] = *it;
            it++;
        }
    }
}

// 拷贝构造
CAGD::Matrix::Matrix(const Matrix &rhs) : m_row(rhs.m_row), m_col(rhs.m_col)
{
    m_value = new double *[m_row];
    for (int i = 0; i < m_row; i++)
    {
        m_value[i] = new double[m_col];
        for (int j = 0; j < m_col; j++)
            m_value[i][j] = rhs.m_value[i][j];
    }
}

CAGD::Matrix CAGD::Matrix::Identity(int n)
{
    assert(n > 0);

    Matrix m(n, n);
    for (int i = 0; i < n; i++)
        m(i, i) = 1;
    return m;
}

CAGD::Matrix CAGD::Matrix::ExchangeRC(int n, int i, int j)
{
    assert(n > 0);

    Matrix m = Identity(n);
    m(i, i) = m(j, j) = 0;
    m(i, j) = m(j, i) = 1;
    return m;
}

CAGD::Matrix CAGD::Matrix::TimesRC(int n, int i, double t)
{
    assert(n > 0);

    Matrix m = Identity(n);
    m(i, i) = t;
    return m;
}

CAGD::Matrix CAGD::Matrix::TimesAddRC(int n, int i, int j, double t)
{
    assert(n > 0);

    Matrix m = Identity(n);
    m(i, j) = t;
    return m;
}

// 获得行数
int CAGD::Matrix::Row() const
{
    return m_row;
}

// 获得列数
int CAGD::Matrix::Col() const
{
    return m_col;
}

// 返回 i,j 元素
double CAGD::Matrix::operator()(int i, int j) const
{
    assert(i < m_row && j < m_col && i >= 0 && j >= 0);
    return m_value[i][j];
}

// 返回 i,j 元素引用
double &CAGD::Matrix::operator()(int i, int j)
{
    assert(i < m_row && j < m_col && i >= 0 && j >= 0);
    return m_value[i][j];
}

// 矩阵加减乘
CAGD::Matrix CAGD::Matrix::operator+(const Matrix &rhs) const
{
    assert(m_row == rhs.m_row && m_col == rhs.m_col);

    Matrix m = *this;
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) += rhs(i, j);
    return m;
}

CAGD::Matrix CAGD::Matrix::operator-(const Matrix &rhs) const
{
    assert(m_row == rhs.m_row && m_col == rhs.m_col);

    Matrix m = *this;
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) -= rhs(i, j);
    return m;
}

// 矩阵乘法
CAGD::Matrix CAGD::Matrix::operator*(const Matrix &rhs) const
{
    assert(m_col == rhs.m_row);

    int Col2 = rhs.m_col;
    Matrix m(m_row, Col2);
    for (int i = 0; i < m_row; i++)
    {
        for (int j = 0; j < Col2; j++)
        {
            double sum = 0;
            for (int k = 0; k < m_col; k++)
                sum += m_value[i][k] * rhs(k, j);
            m(i, j) = sum;
        }
    }
    return m;
}

CAGD::Vector CAGD::Matrix::operator*(const Vector &v) const
{
    assert(m_col == v.Size());

    Vector res(m_row);
    for (int i = 0; i < m_row; i++)
    {
        double sum = 0;
        for (int j = 0; j < m_col; j++)
            sum += m_value[i][j] * v[j];
        res[i] = sum;
    }
    return res;
}

CAGD::Matrix CAGD::Matrix::operator*(double t) const
{
    Matrix m = *this;
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) *= t;
    return m;
}

CAGD::Matrix CAGD::Matrix::operator/(double t) const
{
    assert(fabs(t) > FLT_MIN);

    Matrix m = *this;
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) /= t;
    return m;
}

CAGD::Matrix CAGD::Matrix::operator^(int N) const
{
    int n = m_row;
    assert(m_row == m_col);

    if (N == 0)
        return Matrix::Identity(n);
    if (N < 0)
    {
        int flag;
        auto B = Invert(&flag);

        // 如果矩阵奇异就返回单位阵
        if (flag == -1)
            return Matrix::Identity(n);
        else
            return B ^ (-N);
    }
    return (*this) * ((*this) ^ (N - 1));
}

// 直接赋值
void CAGD::Matrix::operator=(const Matrix &rhs)
{
    assert(m_row == rhs.m_row && m_col == rhs.m_col);

    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m_value[i][j] = rhs(i, j);
}

// 比较
bool CAGD::Matrix::operator==(const Matrix &rhs) const
{
    if (m_row != rhs.m_row || m_col != rhs.m_col)
        return false;

    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            if (fabs(m_value[i][j] - rhs(i, j)) > HALF_ACCURACY)
                return false;
    return true;
}

// 取反操作
CAGD::Matrix CAGD::Matrix::operator-() const
{
    Matrix m = *this;
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) *= -1;
    return m;
}

// 计算范数
double CAGD::Matrix::Norm(int nt) const
{
    const Matrix &lhs = *this;
    if (nt == 1)
    {
        double r = 0;
        for (int i = 0; i < m_col; i++)
        {
            double a = 0;
            for (int j = 0; j < m_row; a += fabs(lhs(j++, i)))
                ;
            r = MAX(a, r);
        }
        return r;
    }
    // 无穷范数
    else if (nt == 0)
    {
        double r = 0;
        for (int i = 0; i < m_row; i++)
        {
            double a = 0;
            for (int j = 0; j < m_col; a += fabs(lhs(i, j++)))
                ;
            r = MAX(a, r);
        }
        return r;
    }
    // 二范数
    else
    {
        // 计算 AA^T 的最大特征值开根
        LinearSolver solver;
        Matrix A = (*this) * this->Transpose();
        double r = sqrt(solver.Power(A));
        return r;
    }
    return 0;
}

// e 范数 / f 范数
double CAGD::Matrix::Norm(char c) const
{
    const Matrix &lhs = *this;
    if (c == 'e')
    {
        double sum = 0;
        for (int i = 0; i < m_row; i++)
            for (int j = 0; j < m_col; j++)
                if (i != j)
                    sum += lhs(i, j) * lhs(i, j);
        return sqrt(sum);
    }
    else if (c == 'f')
    {
        double sum = 0;
        for (int i = 0; i < m_row; i++)
            for (int j = 0; j < m_col; j++)
                sum += lhs(i, j) * lhs(i, j);
        return sqrt(sum);
    }
    return 0;
}

// 计算条件数
double CAGD::Matrix::Condition() const
{
    int n = Row();
    assert(n == Col());

    // 分别分解
    Matrix A = *this;
    Matrix AT = A.Transpose().LU();
    A = A.LU();

    LinearSolver solver;
    Vector x(n, 1.0 / n), w(n), v(n), z(n);
    while (true)
    {
        // get w
        w = solver.SolveLower(AT, x, true);
        w = solver.SolveUpper(AT, w);
        v = w.Sgn();

        // get z
        z = solver.SolveLower(A, v, true);
        z = solver.SolveUpper(A, z);

        // compare infinite z and zT * x
        if (z.Norm(0) <= z * x)
            break;

        // 将 x 设为 z 最大模位置为 1 的单位向量
        x = Vector(n);
        x[z.MaxId()] = 1;
    }

    return Norm() * w.Norm(1);
}

// 计算矩阵行列式
double CAGD::Matrix::Det() const
{
    int n = Row();
    assert(n == Col());

    // 做选主元 LU 分解
    int *perm = new int[n];
    int flag = 0;
    Matrix A = LU(perm, &flag);
    delete[] perm;

    // 检查状态，如果不可逆，直接返回零
    if (flag == -1)
        return 0;

    // 计算对角元素的乘积返回
    double s = 1;
    for (int i = 0; i < n; i++)
        s *= A(i, i);
    return s;
}

double CAGD::Matrix::Trace() const
{
    int n = Row();
    assert(n == Col());

    double s = 0;
    for (int i = 0; i < n; i++)
        s += m_value[i][i];

    return s;
}

// 获得对角向量
CAGD::Vector CAGD::Matrix::Diag() const
{
    Vector d(MIN(m_row, m_col));
    for (int i = 0; i < MIN(m_row, m_col); i++)
        d[i] = m_value[i][i];
    return d;
}

// 获取上三角
CAGD::Matrix CAGD::Matrix::Upper() const
{
    Matrix m(m_row, m_col);
    for (int i = 0; i < MIN(m_row, m_col); i++)
        for (int j = i; j < m_col; j++)
            m(i, j) = m_value[i][j];
    return m;
}

// 获得下三角
CAGD::Matrix CAGD::Matrix::Lower() const
{
    Matrix m(m_row, m_col);
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < MIN(MIN(m_row, m_col), i + 1); j++)
            m(i, j) = m_value[i][j];
    return m;
}

// 转置矩阵
CAGD::Matrix CAGD::Matrix::Transpose() const
{
    Matrix m(m_col, m_row);
    for (int i = 0; i < m_col; i++)
        for (int j = 0; j < m_row; j++)
            m(i, j) = m_value[j][i];
    return m;
}

// 矩阵求逆
CAGD::Matrix CAGD::Matrix::Invert(int *flag) const
{
    assert(m_row == m_col);

    // 初始化单位阵
    Matrix m(m_row, m_row);
    for (int i = 0; i < m_row; i++)
        m(i, i) = 1;

    // 保存当前矩阵
    Matrix old = *this;

    double max; // 最大值
    int index;  // 最大值位置

    for (int i = 0; i < m_row; i++)
    {
        max = 0;
        index = i;

        // 寻找最大列元作为主元
        for (int j = i; j < m_row; j++)
        {
            double tmp = fabs(old(j, i));
            if (tmp > max)
            {
                max = tmp;
                index = j;
            }
        }

        // 确保主元足够大，否则不可逆
        if (max < FLT_MIN)
        {
            if (flag != nullptr)
                *flag = -1;
            return m;
        }

        // 交换最大元到对角位置
        for (int j = 0; j < m_row; j++)
        {
            double tmp = old(i, j);
            old(i, j) = old(index, j);
            old(index, j) = tmp;

            tmp = m(i, j);
            m(i, j) = m(index, j);
            m(index, j) = tmp;
        }

        // Gauss 消去
        for (int j = 0; j < m_row; j++)
        {
            double tmp = old(i, i);
            if (i == j)
            {
                for (int k = 0; k < m_row; k++)
                {
                    m(j, k) /= tmp;
                    old(j, k) /= tmp;
                }
                continue;
            }
            tmp = old(j, i) / old(i, i);
            for (int k = 0; k < m_row; k++)
            {
                m(j, k) -= tmp * m(i, k);
                old(j, k) -= tmp * old(i, k);
            }
        }
    }

    return m;
}

// 列主元 LU 分解，传入下标数组，返回置换后的数组
CAGD::Matrix CAGD::Matrix::LU(int *perm, int *flag) const
{
    assert(m_row == m_col);

    Matrix A = *this;
    for (int i = 0; i < m_row; i++)
    {
        // 只有传入了置换数组时，才进行列主元处理
        if (perm != nullptr)
        {
            double tmp, max = 0; // 记录最大值
            int index = i;       // 记录要交换的主元位置

            // 选取列主元
            for (int p = i; p < m_row; p++)
            {
                tmp = fabs(A(p, i));
                if (tmp > max)
                {
                    max = tmp;
                    index = p;
                }
            }

            // 交换主元
            for (int q = 0; q < m_row; q++)
            {
                tmp = A(i, q);
                A(i, q) = A(index, q);
                A(index, q) = tmp;
            }

            // 交换指标，记录置换
            int tmp_ind = perm[i];
            perm[i] = perm[index];
            perm[index] = tmp_ind;
        }

        // 正常的高斯消去法
        for (int j = i + 1; j < m_row; j++)
        {
            // 确保对角元足够大，否则不可逆
            if (fabs(A(i, i)) < FLT_MIN)
            {
                if (flag != nullptr)
                    *flag = -1;
                return A;
            }

            A(j, i) /= A(i, i);
            for (int k = i + 1; k < m_row; k++)
                A(j, k) -= A(i, k) * A(j, i);
        }
    }

    return A;
}

// Cholesky
CAGD::Matrix CAGD::Matrix::Cholesky(int *flag) const
{
    assert(m_row == m_col);

    Matrix A = *this;
    for (int i = 0; i < m_row; i++)
    {
        double sum = 0;
        for (int p = 0; p < i; p++)
            sum += A(i, p) * A(i, p);
        A(i, i) = sqrt(A(i, i) - sum);

        // 确保对角元足够大
        if (fabs(A(i, i)) < FLT_MIN)
        {
            if (flag != nullptr)
                *flag = -1;
            return A;
        }

        for (int j = i + 1; j < m_row; j++)
        {
            sum = 0;
            for (int k = 0; k < i; k++)
                sum += A(j, k) * A(i, k);

            A(j, i) = (A(j, i) - sum) / A(i, i);
            A(i, j) = A(j, i);
        }
    }
    return A;
}

// 优化的 Cholesky
CAGD::Matrix CAGD::Matrix::CholeskyD(int *flag) const
{
    assert(m_row == m_col);

    Matrix A = *this;
    for (int i = 0; i < m_row; i++)
    {
        double sum = 0;
        for (int p = 0; p < i; p++)
            sum += A(i, p) * A(i, p) * A(p, p);

        A(i, i) -= sum;

        // 确保对角元足够大
        if (fabs(A(i, i)) < FLT_MIN)
        {
            if (flag != nullptr)
                *flag = -1;
            return A;
        }

        for (int j = i + 1; j < m_row; j++)
        {
            sum = 0;
            for (int k = 0; k < i; k++)
                sum += A(j, k) * A(i, k) * A(k, k);

            A(j, i) -= sum;
            A(j, i) /= A(i, i);
            A(i, j) = A(j, i);
        }
    }

    return A;
}

// QR 分解
CAGD::Matrix CAGD::Matrix::QR(Vector &d) const
{
    assert(d.Size() == m_col);

    Matrix A = *this;
    for (int i = 0; i < m_col; i++)
    {
        Vector x(m_row - i);
        Vector v(m_row - i);

        for (int j = i; j < m_row; j++)
            x[j - i] = A(j, i);

        double beta = Vector::HouseHolder(x, v);
        Vector w(m_col - i);

        // get w = beta * AT * v
        for (int k = i; k < m_col; k++)
        {
            double sum = 0;
            for (int p = i; p < m_row; p++)
                sum += A(p, k) * v[p - i];

            // note: it is w[k-i], not w[k]
            w[k - i] = beta * sum;
        }

        // get HA = A - v * wT
        for (int k = i; k < m_row; k++)
        {
            for (int p = i; p < m_col; p++)
            {
                if (p == i && k > i)
                    A(k, p) = v[k - i];
                else
                    A(k, p) -= v[k - i] * w[p - i];
            }
        }

        d[i] = beta;
    }

    return A;
}

// 上 Hessenberg 化，在 Q 中累计正交变换
CAGD::Matrix CAGD::Matrix::Hessenberg(Matrix *Q) const
{
    assert(m_row == m_col);

    Matrix A = *this;
    int n = m_row;
    for (int i = 0; i < n - 2; i++)
    {
        // 用于保存 Vector::HouseHolder 变换
        Vector x(n - i - 1);
        Vector v(n - i - 1);

        // x 取 A 的 i 列对角元以下的部分
        for (int j = i + 1; j < n; j++)
            x[j - i - 1] = A(j, i);

        // 为了节约，将 beta 存放在 v 头部
        double beta = Vector::HouseHolder(x, v);
        v[0] = beta;

        // 注意列的范围要从 0 到 n，不然右上角的元素无法更新
        A.HA(i + 1, n, v);
        A.AH(i + 1, n, v);

        // 累计正交变换
        if (Q != nullptr)
            Q->AH(i + 1, n, v);
    }

    return A;
}

// 对称阵化为三对角对称阵，在 Q 中累计正交变换
CAGD::Matrix CAGD::Matrix::Tridiagonal(Matrix *Q) const
{
    assert(m_row == m_col);

    Matrix A = *this;
    int n = m_row;
    for (int i = 0; i < n - 2; i++)
    {
        // 用于保存 Vector::HouseHolder 变换
        Vector x(n - i - 1);
        Vector v(n - i - 1);

        // x 取 A 的 i 列对角元以下的部分
        for (int j = i + 1; j < n; j++)
            x[j - i - 1] = A(j, i);

        // 为了节约，将 beta 存放在 v 头部
        double beta = Vector::HouseHolder(x, v);
        v[0] = beta;

        // 累计正交变换
        if (Q != nullptr)
            Q->AH(i + 1, n, v);

        // 注意列的范围要从 0 到 n，不然右上角的元素无法更新
        A.HAHs(i + 1, n, v);
    }

    return A;
}

// 矩阵上二对角化，在 U,V 中累计正交变换
CAGD::Matrix CAGD::Matrix::Didiagonal(Matrix *U, Matrix *V) const
{
    Matrix A = *this;
    int n = A.Col();
    int m = A.Row();
    // m >= n
    for (int i = 0; i < n; i++)
    {
        // 用于保存 Vector::HouseHolder 变换
        Vector x(m - i);
        Vector v(m - i);

        // x 取 A 的 i 列对角元和以下的部分
        for (int j = i; j < m; j++)
            x[j - i] = A(j, i);

        double beta = Vector::HouseHolder(x, v);
        v[0] = beta;

        // 累计正交变换
        if (U != nullptr)
            U->AH(i, m, v);

        A.HA(i, m, v);

        if (i < n - 2)
        {
            // 用于保存 Vector::HouseHolder 变换
            Vector x(n - i - 1);
            Vector v(n - i - 1);

            // x 取 A 的 i 行对角元以下的部分
            for (int j = i + 1; j < n; j++)
                x[j - i - 1] = A(i, j);

            double beta = Vector::HouseHolder(x, v);
            v[0] = beta;

            // 累计正交变换
            if (V != nullptr)
                V->AH(i + 1, n, v);

            A.AH(i + 1, n, v);
        }
    }

    return A;
}

// 计算指定范围的 Vector::HouseHolder 变换，作用结果为 HA
void CAGD::Matrix::HA(int startOfRow, int endOfRow, Vector &v)
{
    Matrix &A = *this;
    int n = A.Col();

    // 先获得变换的变量
    double beta = v[0];
    v[0] = 1;

    // 逐列计算 HA
    for (int j = 0; j < n; j++)
    {
        double sum = 0;
        // 计算 beta * vT * Aj
        for (int k = startOfRow; k < endOfRow; k++)
            sum += beta * v[k - startOfRow] * A(k, j);

        // 计算 Aj - v * beta * vT * Aj
        for (int k = startOfRow; k < endOfRow; k++)
            A(k, j) -= sum * v[k - startOfRow];
    }

    // 最后还原 v[0]
    v[0] = beta;
}

// 计算指定范围的 Vector::HouseHolder 变换，作用结果为 AH
void CAGD::Matrix::AH(int startOfCol, int endOfCol, Vector &v)
{
    Matrix &A = *this;
    int n = A.Row();

    // 先获得变换的变量
    double beta = v[0];
    v[0] = 1;

    // 逐行计算 AH
    for (int j = 0; j < n; j++)
    {
        double sum = 0;
        // 计算 beta * ATj * v
        for (int k = startOfCol; k < endOfCol; k++)
            sum += beta * v[k - startOfCol] * A(j, k);

        // 计算 ATj - beta * ATj * v * vT
        for (int k = startOfCol; k < endOfCol; k++)
            A(j, k) -= sum * v[k - startOfCol];
    }

    // 最后还原 v[0]
    v[0] = beta;
}

void CAGD::Matrix::HAHs(int startOfRow, int endOfRow, Vector &v)
{
    assert(m_row == m_col);

    // HAH = A - v *wT - w * vT
    double beta = v[0];
    v[0] = 1;
    Vector u(endOfRow - startOfRow), w(endOfRow - startOfRow);

    // u = beta * A * v
    Matrix &A = *this;
    for (int i = startOfRow; i < endOfRow; i++)
    {
        double sum = 0;
        for (int j = startOfRow; j < endOfRow; j++)
            sum += A(i, j) * v[j - startOfRow];

        u[i - startOfRow] = beta * sum;
    }

    // w = u - 1/2 * beta * (vT *u) * v
    w = u - v * (v * u) * beta * 0.5;

    // Vector::HouseHolder 变换后，次对角应当变成这一列的模长，同时这一列其它元素归零
    double sum = 0;
    for (int i = startOfRow; i < endOfRow; i++)
    {
        sum += A(i, startOfRow - 1) * A(i, startOfRow - 1);
        A(i, startOfRow - 1) = A(startOfRow - 1, i) = 0;
    }
    A(startOfRow - 1, startOfRow) = A(startOfRow, startOfRow - 1) = sqrt(sum);

    // 计算 A - vw^T - wv^T
    for (int i = startOfRow; i < endOfRow; i++)
        for (int j = startOfRow; j < endOfRow; j++)
            A(i, j) -= v[i - startOfRow] * w[j - startOfRow] + v[j - startOfRow] * w[i - startOfRow];

    // 最后还原 v[0]
    v[0] = beta;
}

// 计算指定分量的 Givens 变换，得到 GA
void CAGD::Matrix::GA(int i, int j, Vector &cs)
{
    Matrix &A = *this;
    int n = A.Col();

    // 计算 G*A
    for (int k = 0; k < n; k++)
    {
        double tmp = A(i, k);
        A(i, k) = tmp * cs[0] + A(j, k) * cs[1];
        A(j, k) = -cs[1] * tmp + A(j, k) * cs[0];
    }
}

// 计算指定分量的 Givens 变换，得到 AG
void CAGD::Matrix::AG(int i, int j, Vector &cs)
{
    Matrix &A = *this;
    int n = A.Row();

    for (int k = 0; k < n; k++)
    {
        double tmp = A(k, i);
        A(k, i) = tmp * cs[0] - A(k, j) * cs[1];
        A(k, j) = cs[1] * tmp + A(k, j) * cs[0];
    }
}

// 计算指定分量的 Givens 变换，得到 AGT
void CAGD::Matrix::AGT(int i, int j, Vector &cs)
{
    Matrix &A = *this;
    int n = A.Row();

    for (int k = 0; k < n; k++)
    {
        double tmp = A(k, i);
        A(k, i) = tmp * cs[0] + A(k, j) * cs[1];
        A(k, j) = -cs[1] * tmp + A(k, j) * cs[0];
    }
}

// 双重步 QR 迭代
void CAGD::Matrix::DoubleQR(int l, int m, Matrix *Q)
{
    assert(m_row == m_col && l < m_row && m < m_col);

    Matrix &A = *this;
    double s = A(m_row - 2, m_row - 2) + A(m_row - 1, m_row - 1);
    double t = A(m_row - 2, m_row - 2) * A(m_row - 1, m_row - 1) - A(m_row - 2, m_row - 1) * A(m_row - 1, m_row - 2);

    Vector x(3);
    x[0] = A(0, 0) * A(0, 0) + A(0, 1) * A(1, 0) - s * A(0, 0) + t;
    x[1] = A(1, 0) * (A(0, 0) + A(1, 1) - s);
    x[2] = A(1, 0) * A(2, 1);
    // 不考虑 n<3 的情况，即 A 至少是 3 阶矩阵

    // 计算 P0 * H * P0
    Vector v(3);
    double beta = Vector::HouseHolder(x, v);
    v[0] = beta;

    // 累计正交变换
    if (Q != nullptr)
        Q->AH(l, l + 3, v);

    // 计算 P0 * H * P0
    HA(l, l + 3, v);
    AH(l, l + 3, v);

    // 开始双重步迭代
    x[0] = A(l + 1, l);
    x[1] = A(l + 2, l);
    for (int i = l + 1; i < m_row - m - 2; i++)
    {
        x[2] = A(i + 2, i - 1);
        if (x.Norm(0) > ACCURACY)
            beta = Vector::HouseHolder(x, v);
        else
            beta = 0;
        v[0] = beta;

        // 累计正交变换
        if (Q != nullptr)
            Q->AH(i, i + 3, v);

        HA(i, i + 3, v);
        AH(i, i + 3, v);

        x[0] = A(i + 1, i);
        x[1] = A(i + 2, i);
    }

    // 最后一步只有 2 阶
    Vector y({x[0], x[1]});
    Vector v2(2);
    beta = Vector::HouseHolder(y, v2);
    v2[0] = beta;

    // 累计正交变换
    if (Q != nullptr)
        Q->AH(m_row - m - 2, m_row - m, v2);

    HA(m_row - m - 2, m_row - m, v2);
    AH(m_row - m - 2, m_row - m, v2);
}

// Wilkinson QR 迭代，针对三对角对称矩阵
void CAGD::Matrix::WilkinsonQR(int l, int m, Matrix *Q)
{
    assert(m_row == m_col && l < m_row && m < m_col);

    Matrix &T = *this;
    int n = m_row;
    int last = n - m - 1;

    // 计算 Wilkinson 位移
    double d = (T(last - 1, last - 1) - T(last, last)) / 2;
    double mu = T(last, last) - T(last, last - 1) * T(last, last - 1) /
                                    (d + SIGN(d) * sqrt(d * d + T(last, last - 1) * T(last, last - 1)));

    Vector xz({T(l, l) - mu, T(l + 1, l)});
    Vector cs(2);
    for (int i = l; i < last; i++)
    {
        // 计算 Givens 变换的系数
        cs = Vector::Givens(xz);

        // 计算 G*T
        T.GA(i, i + 1, cs);
        T.AGT(i, i + 1, cs);

        // 累计正交变换 Q * G
        if (Q != nullptr)
            Q->AGT(i, i + 1, cs);

        // 只要不是最后一行，就获取相邻两行
        if (i < last - 1)
        {
            xz[0] = T(i + 1, i);
            xz[1] = T(i + 2, i);
        }
    }
}

// Wilkinson 位移的 SVD 迭代
void CAGD::Matrix::WilkinsonSVD(int p, int q, Matrix *U, Matrix *V)
{
    assert(p < m_row && q < m_col);

    Matrix &B = *this;
    int n = m_col;
    int last = n - q - 1;

    // 计算 Wilkinson 位移
    // 注意如果 n-p-q，那么 gamma_{n-2} 设为零
    double gamma = (n - p - q > 2) ? B(last - 2, last - 1) : 0;
    double alpha = pow(B(last, last), 2) + pow(B(last - 1, last), 2);
    double delta = (pow(B(last - 1, last - 1), 2) + gamma * gamma - alpha) / 2;
    double beta = B(last - 1, last - 1) * B(last - 1, last);
    double mu = alpha - beta * beta / (delta + SIGN(delta) * sqrt(delta * delta + beta * beta));

    // 计算 Givens 变换的系数，注意第一步 Givens 变换右乘，需要反转变换矩阵
    Vector yz({B(p, p) * B(p, p) - mu, B(p, p) * B(p, p + 1)});
    Vector cs = Vector::Givens(yz);
    cs[1] *= -1;

    // 累计正交变换
    if (V != nullptr)
        V->AG(p, p + 1, cs);

    // B * G
    B.AG(p, p + 1, cs);

    // 记录 Givens 变换
    for (int i = p; i < last; i++)
    {
        // 更新 yz
        yz[0] = B(i, i);
        yz[1] = B(i + 1, i);

        // Givens 行变换
        cs = Vector::Givens(yz);

        // 累计正交变换
        if (U != nullptr)
            U->AGT(i, i + 1, cs);

        // G * B
        B.GA(i, i + 1, cs);

        // 只要不是最后一行，就获取相邻两列计算
        if (i < last - 1)
        {
            // 更新 yz
            yz[0] = B(i, i + 1);
            yz[1] = B(i, i + 2);

            // Givens 列变换
            cs = Vector::Givens(yz);

            // 累计正交变换
            if (V != nullptr)
                V->AG(i + 1, i + 2, cs);

            B.AG(i + 1, i + 2, cs);
        }
    }
}

// 沿着 p,q 分量旋转得到 JT*A*J, Q 累计 J
void CAGD::Matrix::Rotate(Matrix &Q, int p, int q)
{
    assert(m_row == m_col);

    Matrix &A = *this;
    int n = A.Row();

    double cs[2];
    if (A(p, q) == 0)
    {
        cs[0] = 1;
        cs[1] = 0;
    }
    else
    {
        // t*t + 2*tau*t - 1 = 0
        double tau = (A(q, q) - A(p, p)) / (2 * A(p, q));
        double t = SIGN(tau);
        t /= (fabs(tau) + sqrt(1 + tau * tau));
        cs[0] = 1 / (sqrt(1 + t * t));
        cs[1] = t * cs[0];
    }
    // JT * A
    for (int j = 0; j < n; j++)
    {
        double tmp;
        tmp = A(p, j);
        A(p, j) = tmp * cs[0] - A(q, j) * cs[1];
        A(q, j) = cs[1] * tmp + A(q, j) * cs[0];
    }
    // JT * A * J, note: here J is not a symmetical matrix
    // Q * J
    for (int j = 0; j < n; j++)
    {
        double tmp;
        tmp = A(j, p);
        A(j, p) = tmp * cs[0] - A(j, q) * cs[1];
        A(j, q) = cs[1] * tmp + A(j, q) * cs[0];

        tmp = Q(j, p);
        Q(j, p) = tmp * cs[0] - Q(j, q) * cs[1];
        Q(j, q) = cs[1] * tmp + Q(j, q) * cs[0];
    }
}

// 释放资源
CAGD::Matrix::~Matrix()
{
    for (int j = 0; j < m_row; j++)
        delete[] m_value[j];
    delete[] m_value;
}

// 基本构造函数
CAGD::Sparse::Sparse(int row, int col) : m_row(row), m_col(col)
{
    m_IA = new int[row + 1];
    for (int i = 0; i < m_row; i++)
        m_IA[i] = 0;

    // 有效元素个数为零
    m_IA[row] = 0;

    // 只需要构造边界值
    m_JA.push_back(INT_MAX);
    m_AA.push_back(FLT_MAX);
}

// 拷贝构造
CAGD::Sparse::Sparse(const Sparse &spm) : m_row(spm.m_row), m_col(spm.m_col), m_JA(spm.m_JA), m_AA(spm.m_AA)
{
    m_IA = new int[m_row + 1];
    for (int i = 0; i < m_row + 1; i++)
        m_IA[i] = spm.m_IA[i];
}

// 获得矩阵列数
int CAGD::Sparse::Col() const
{
    return m_col;
}

// 获得矩阵的行数
int CAGD::Sparse::Row() const
{
    return m_row;
}

// 获得有效元素的个数
int CAGD::Sparse::Nnz() const
{
    return m_IA[m_row];
}

// 重载以获得指定位置的元素，只能获得存在的元素，不存在的元素认为是零
double CAGD::Sparse::operator()(int i, int j) const
{
    assert(i < m_row);
    assert(j < m_col);

    for (int k = m_IA[i]; k < m_IA[i + 1]; k++)
    {
        if (m_JA[k] == j)
            return m_AA[k];
    }
    return 0;
}

// 设置指定位置的元素，不存在就创建新元素的引用返回
void CAGD::Sparse::Set(int i, int j, double value)
{
    assert(i < m_row);
    assert(j < m_col);

    // 如果找到了就直接设置，然后立即返回
    for (int k = m_IA[i]; k < m_IA[i + 1]; k++)
    {
        if (m_JA[k] == j)
        {
            m_AA[k] = value;
            return;
        }
    }

    // 否则，就插入新元素；找到第一个元素，其列标大于 j，则它是插入位置
    int k = m_IA[i];
    for (; k < m_IA[i + 1]; k++)
    {
        if (m_JA[k] > j)
            break;
    }

    // 插入列标和元素
    auto it_J = m_JA.begin() + k;
    auto it_A = m_AA.begin() + k;
    it_J = m_JA.insert(it_J, j);
    it_A = m_AA.insert(it_A, value);

    // 将 i 行后记录的索引都加 1，注意也包括最后一个元素
    for (int k = i + 1; k < m_row + 1; k++)
        m_IA[k]++;
}

// 在指定位置加
void CAGD::Sparse::Add(int i, int j, double value)
{
    assert(i < m_row);
    assert(j < m_col);

    // 如果找到了就直接设置，然后立即返回
    for (int k = m_IA[i]; k < m_IA[i + 1]; k++)
    {
        if (m_JA[k] == j)
        {
            m_AA[k] += value;
            return;
        }
    }

    // 否则，就插入新元素；找到第一个元素，其列标大于 j，则它是插入位置
    int k = m_IA[i];
    for (; k < m_IA[i + 1]; k++)
    {
        if (m_JA[k] > j)
            break;
    }

    // 插入列标和元素
    auto it_J = m_JA.begin() + k;
    auto it_A = m_AA.begin() + k;
    it_J = m_JA.insert(it_J, j);
    it_A = m_AA.insert(it_A, value);

    // 将 i 行后记录的索引都加 1，注意也包括最后一个元素
    for (int k = i + 1; k < m_row + 1; k++)
        m_IA[k]++;
}

// 拷贝赋值
void CAGD::Sparse::operator=(const Sparse &spm)
{
    assert(m_row == spm.m_row);
    assert(m_col == spm.m_col);

    delete[] m_IA;
    m_IA = new int[m_row + 1];
    for (int i = 0; i < m_row + 1; i++)
        m_IA[i] = spm.m_IA[i];

    m_JA = spm.m_JA;
    m_AA = spm.m_AA;
}

// 加减法
CAGD::Sparse CAGD::Sparse::operator+(const Sparse &spm) const
{
    assert(spm.Row() == m_row);
    assert(spm.Col() == m_col);

    Sparse res = *this;
    for (int i = 0; i < m_row; i++)
    {
        // IA[m] = nnz，边界值
        for (int j = spm.m_IA[i]; j < spm.m_IA[i + 1]; j++)
            res.Add(i, spm.m_JA[j], spm.m_AA[j]);
    }
    return res;
}

CAGD::Sparse CAGD::Sparse::operator-(const Sparse &spm) const
{
    assert(spm.Row() == m_row);
    assert(spm.Col() == m_col);

    Sparse res = *this;
    for (int i = 0; i < m_row; i++)
    {
        // IA[m] = nnz，边界值
        for (int j = spm.m_IA[i]; j < spm.m_IA[i + 1]; j++)
            res.Add(i, spm.m_JA[j], -spm.m_AA[j]);
    }
    return res;
}

// 乘除法运算
CAGD::Sparse CAGD::Sparse::operator*(double k) const
{
    Sparse res = *this;
    // 除了最后一个元素以外都乘 k
    for (int i = 0; i < m_AA.size() - 1; i++)
        res.m_AA[i] *= k;
    return res;
}

CAGD::Sparse CAGD::Sparse::operator/(double k) const
{
    assert(fabs(k) > FLT_MIN);

    Sparse res = *this;
    for (int i = 0; i < m_AA.size() - 1; i++)
        res.m_AA[i] /= k;
    return res;
}

CAGD::Vector CAGD::Sparse::operator*(const Vector &v) const
{
    assert(m_col == v.Size());

    Vector res(m_row);
    for (int i = 0; i < m_row; i++)
    {
        // 计算 i 行的结果
        for (int j = m_IA[i]; j < m_IA[i + 1]; j++)
        {
            int k = m_JA[j];
            res[i] += m_AA[j] * v[k];
        }
    }
    return res;
}

// 稀疏矩阵的乘法
CAGD::Sparse CAGD::Sparse::operator*(const Sparse &spm) const
{
    assert(m_col == spm.Row());

    Sparse res(m_row, spm.m_col);
    int Col2 = spm.m_col;
    for (int i = 0; i < Row(); ++i)
    {
        // 暂存数据
        double *tmp_val = new double[Col2];
        for (int j = 0; j < Col2; j++)
            tmp_val[j] = 0;

        // 计算 i 行的结果
        for (int j = m_IA[i]; j < m_IA[i + 1]; j++)
        {
            // 获得对应 JA[IA[i]] 列的 spm 的 JA[IA[i]] 行元素中非零列标索引
            int start = spm.m_IA[m_JA[j]];
            int end = spm.m_IA[m_JA[j] + 1];

            // 这是一个很巧妙的计算方式，只需要每次计算两个元素的乘积，记录在对应的列的位置上
            // 然后不断累加即可，相当于将n次乘法和加法分开计算
            for (int k = start; k < end; k++)
                tmp_val[spm.m_JA[k]] += m_AA[j] * spm.m_AA[k];
        }

        // 去掉过小的元素
        for (int j = 0; j < Col2; j++)
            if (fabs(tmp_val[j]) > ACCURACY)
                res.Set(i, j, tmp_val[j]);

        delete[] tmp_val;
    }
    return res;
}

// 转换为一般矩阵
CAGD::Matrix CAGD::Sparse::ToBMatrix() const
{
    Matrix res(m_row, m_col);
    for (int i = 0; i < m_row; i++)
    {
        // IA[m] = nnz，边界值
        for (int j = m_IA[i]; j < m_IA[i + 1]; j++)
            res(i, m_JA[j]) = m_AA[j];
    }
    return res;
}

// 转置矩阵
CAGD::Sparse CAGD::Sparse::Transpose() const
{
    Sparse res(m_col, m_row);
    for (int i = 0; i < m_row; i++)
    {
        // IA[m] = nnz，边界值
        for (int j = m_IA[i]; j < m_IA[i + 1]; j++)
            res.Set(m_JA[j], i, m_AA[j]);
    }
    return res;
}

// 释放资源
CAGD::Sparse::~Sparse()
{
    delete[] m_IA;
}

// 是否按照单位上三角阵求解
CAGD::Vector CAGD::LinearSolver::SolveUpper(const Matrix &A, const Vector &b, bool unit)
{
    int n = b.Size();
    Vector x(n);
    for (int i = n - 1; i >= 0; i--)
    {
        double sum = 0;
        for (int j = i + 1; j < n; j++)
            sum += A(i, j) * x[j];
        x[i] = (b[i] - sum) / (unit ? 1 : A(i, i));
    }
    return x;
}

// 是否按照单位下三角阵求解
CAGD::Vector CAGD::LinearSolver::SolveLower(const Matrix &A, const Vector &b, bool unit)
{
    int n = b.Size();
    Vector x(n);
    x[0] = b[0] / (unit ? 1 : A(0, 0));
    for (int i = 1; i < n; i++)
    {
        double sum = 0;
        for (int j = 0; j < i; j++)
            sum += A(i, j) * x[j];
        x[i] = (b[i] - sum) / (unit ? 1 : A(i, i));
    }
    return x;
}

// 是否选主元求解
CAGD::Vector CAGD::LinearSolver::Solve(const Matrix &A, const Vector &b, bool choose)
{
    int n = b.Size();
    Vector x(n);

    if (choose)
    {
        // 记录置换
        int *perm = new int[n];
        for (int i = 0; i < n; i++)
            perm[i] = i;

        Matrix B = A.LU(perm);

        // 对 b 进行置换
        Vector b1(n);
        for (int i = 0; i < n; i++)
            b1[i] = b[perm[i]];

        x = SolveLower(B, b1, true);
        x = SolveUpper(B, x);

        delete[] perm;
    }
    else
    {
        Matrix B = A.LU();

        x = SolveLower(B, b, true);
        x = SolveUpper(B, x);
    }

    return x;
}

// 是否优化为 LDL 分解
CAGD::Vector CAGD::LinearSolver::Cholesky(const Matrix &A, const Vector &b, bool improve)
{
    int n = b.Size();
    Vector x(n);

    if (improve)
    {
        Matrix B = A.CholeskyD();

        // 将对角元乘到上三角部分
        for (int i = 0; i < n; i++)
            for (int j = i + 1; j < n; j++)
                B(i, j) *= B(i, i);

        x = SolveLower(B, b, true);
        x = SolveUpper(B, x);
    }
    else
    {
        Matrix B = A.Cholesky();

        x = SolveLower(B, b);
        x = SolveUpper(B, x);
    }
    return x;
}

// QR 分解法
CAGD::Vector CAGD::LinearSolver::QR(const Matrix &A, const Vector &t)
{
    int row = A.Row();
    int col = A.Col();
    Vector d(col);
    Matrix B = A.QR(d);
    Vector b = t;

    // Householder 变换作用于 b
    // Hb = b - beta * v * v^T * b = b - (beta * v^T * b) * v
    for (int i = 0; i < col; i++)
    {
        double sum = b[i];
        for (int j = i + 1; j < row; j++)
            sum += B(j, i) * b[j];

        sum *= d[i];
        b[i] -= sum;
        for (int j = i + 1; j < row; j++)
            b[j] -= B(j, i) * sum;
    }

    // 求解上半部分的线性方程组
    Vector b1(col);
    for (int i = 0; i < col; i++)
        b1[i] = b[i];

    return SolveUpper(B, b1);
}

// 共轭梯度法
void CAGD::LinearSolver::CG(const Matrix &A, const Vector &b, Vector &x)
{
    // 只能用于方阵
    assert(A.Row() == A.Col());

    int n = A.Col();
    int count = 0;

    Vector p = b - A * x;
    Vector r = p;

    double rr = r * r; // get r0 * r0, next r0 * r0

    while (sqrt(rr) > ACCURACY && count < ITERATION)
    {
        double alpha = rr / (p * (A * p)); // get alpha_0 = r0T * r0 / p0T * A * p0
        x = x + p * alpha;                 // get xk = xk-1 + alphak-1 * pk-1
        r = r - A * p * alpha;             // get r = rk-1 - alphak-1 * A * pk-1
        double nrr = r * r;                // get next r * r, nrr = r * r
        double beta = nrr / rr;            // get beta = rkT * rk / rk-1T * rk-1
        rr = nrr;                          // get p = rk + betak-1 * pk-1
        p = r + p * beta;                  // get alpha
        count++;
    }
}

// 幂法计算 A-muI 的最大特征值，然后加回 mu
double CAGD::LinearSolver::Power(const Matrix &A, double mu)
{
    assert(A.Row() == A.Col());

    int n = A.Row();
    Vector x(n);
    x[0] = 1;
    Matrix B = A;
    for (int i = 0; i < n; i++)
        B(i, i) -= mu;

    double dp = 1;
    double q = 0;

    Vector y(n);
    // 精度要求不能太高，不然算不出来，实际上 1e-5 就可以
    int count = 0;
    while (dp > ACCURACY && count < ITERATION)
    {
        y = B * x;
        // 选择模最大的分量
        double p = y[y.MaxId()];
        x = y / p;
        dp = fabs(p - q);
        q = p;
        count++;
    }

    return q + mu;
}

// 反幂法计算指定特征值的特征向量
CAGD::Vector CAGD::LinearSolver::InversePower(const Matrix &A, double lambda)
{
    assert(A.Row() == A.Col());

    int n = A.Row();
    Vector e(n, 1);
    Matrix B = A;
    for (int i = 0; i < n; i++)
        B(i, i) -= lambda;

    // 半次迭代法
    int *p = new int[n];
    B = B.LU(p);
    delete[] p;

    e = SolveUpper(B, e);
    return e / e.Norm();
}

// Jacobi 迭代法
CAGD::Vector CAGD::LinearSolver::Jacobi(const Matrix &A, const Vector &b, double omega)
{
    assert(A.Row() == A.Col());

    // x = D1(L+U) * x + D1 * g
    int n = A.Row();
    Vector x(n), y(n);
    double dx = 0;

    do
    {
        // 分步迭代过程
        for (int i = 0; i < n; i++)
        {
            double sum = 0;
            for (int j = 0; j < n; j++)
                if (i != j)
                    sum -= A(i, j) * x[j];

            y[i] = (1 - omega) * x[i] + omega * (sum + b[i]) / A(i, i);
        }
        dx = (y - x).Norm(0);
        x = y;
    } while (fabs(dx) >= ACCURACY);

    return x;
}

// Gauss-Sediel 迭代
CAGD::Vector CAGD::LinearSolver::GaussSediel(const Matrix &A, const Vector &b)
{
    assert(A.Row() == A.Col());

    int n = A.Row();
    Vector x(n);
    double dx = 0;

    do
    {
        // 每次刷新误差
        dx = 0;

        // 分部迭代过程
        for (int i = 0; i < n; i++)
        {
            double sum = 0;
            for (int j = 0; j < n; j++)
                if (j != i)
                    sum -= A(i, j) * x[j];

            double tmp = x[i];
            x[i] = (sum + b[i]) / A(i, i);

            // 注意这里只记录了一个分量的误差
            dx = MAX(fabs(tmp - x[i]), dx);
        }
    } while (dx >= ACCURACY);

    return x;
}

// 超松弛迭代
CAGD::Vector CAGD::LinearSolver::SOR(const Matrix &A, const Vector &b, double w)
{
    // x = Lw * x + w(D-wL)1 * b
    assert(A.Row() == A.Col());

    int n = A.Row();
    Vector x(n);
    double dx = 0;

    do
    {
        // 每次刷新误差
        dx = 0;

        // 分部迭代过程
        for (int i = 0; i < n; i++)
        {
            double sum = 0;
            for (int j = 0; j < n; j++)
                if (j != i)
                    sum -= A(i, j) * x[j];

            double tmp = x[i];
            x[i] = (1 - w) * x[i] + w * (sum + b[i]) / A(i, i);

            // 注意这里只记录了一个分量的误差
            dx = MAX(fabs(tmp - x[i]), dx);
        }
    } while (dx >= ACCURACY);

    return x;
}

// 隐式 QR 方法
CAGD::Matrix CAGD::LinearSolver::ImplicitQR(const Matrix &A, Matrix *Q, bool ifSym)
{
    assert(A.Row() == A.Col());

    int l, m, n = A.Row(), count = 0;
    Matrix B = ifSym ? A.Tridiagonal(Q) : A.Hessenberg(Q);

    // 限制最多迭代 100 次，指定一个较小的比较阈值
    // 这里使用 HALF_ACCURACY 是因为当约化到只剩下 3x3 矩阵时，很有可能出现下面的情况
    // 1.25 8.81 5.12
    // HALF_ACCURACY 1.78 6.25
    // 1e-25 2.14 3.96
    // 注意向量值几乎集中在一个元素上，Householder 变换中存在 sigma = dot(x,x) - x[0] * x[0]
    // 而 HALF_ACCURACY * HALF_ACCURACY = 1e-12 可能小于 ACCURACY，这时候无论是 3 阶还是 2 阶 Householder
    // 变换都会返回一个单位变换 这时候继续迭代已经没有意义，取 HALF_ACCURACY 只是通过观察得到的结果
    double threshold = HALF_ACCURACY;
    while (count < ITERATION)
    {
        // 将足够小的元素归零
        for (int i = 1; i < n; i++)
        {
            if (fabs(B(i, i - 1)) <= ((fabs(B(i - 1, i - 1)) + fabs(B(i, i))) * threshold))
            {
                B(i, i - 1) = 0;
                ifSym ? B(i - 1, i) = 0 : 0;
            }
        }

        // 划分成三部分
        m = 0;

        if (ifSym)
        {
            // 检查对角阵的范围
            for (int i = n - 1; i > 0; i--)
            {
                // 检查次对角元，如果很大，说明对角阵到这里结束
                if (fabs(B(i, i - 1)) > ((fabs(B(i - 1, i - 1)) + fabs(B(i, i))) * threshold))
                    break;

                // 已经到达第二行，说明是对角阵，迭代完成
                if (i - 1 == 0)
                    return B;

                m++;
            }
        }
        else
        {
            // 从最后一行向上，检查拟上三角阵的范围，即对角块为 1x1 或 2x2 的矩阵
            double last = B(n - 1, n - 2);
            for (int i = n - 2; i > 0; i--)
            {
                // 检查两个相邻的次对角元，如果都很大，说明拟三角阵到这里结束
                double compare = (fabs(B(i - 1, i - 1)) + fabs(B(i, i))) * threshold;
                if (fabs(B(i, i - 1)) > compare && fabs(last) > compare)
                    break;

                // 已经到达第二行，说明全是拟上三角，迭代完成，直接返回
                if (i - 1 == 0)
                    return B;

                // 更新次对角
                last = B(i, i - 1);
                m++;
            }
        }

        // 从拟上三角阵头部向上，检查不可约上 Hessenberg 阵的范围
        l = 1;
        for (int i = n - m - 1; i > 0; i--)
            if (fabs(B(i, i - 1)) > ((fabs(B(i - 1, i - 1)) + fabs(B(i, i))) * threshold))
                l++;

        // 得到上面未约化的矩阵阶数
        l = n - m - l;

        // 对中间的不可约部分进行双重步迭代/Wilkinson 迭代
        if (ifSym)
            B.WilkinsonQR(l, m, Q);
        else
            B.DoubleQR(l, m, Q);

        count++;
    }
    return B;
}

// 过关 Jacobi 方法
CAGD::Matrix CAGD::LinearSolver::PassJacobi(Matrix &A, double sigma)
{
    assert(A.Row() == A.Col());

    // Q 初始化为单位阵，用于累计正交变换
    int n = A.Row();
    Matrix Q(n, n);
    for (int i = 0; i < n; i++)
        Q(i, i) = 1;

    // delta_k = delta_{k-1} / sigma
    if (sigma < n)
        return Q;

    double delta = A.Norm('e');
    int count = 0;
    while (delta > ACCURACY && count < ITERATION)
    {
        bool check = true;
        for (int i = 0; i < n; i++)
        {
            for (int j = i + 1; j < n; j++)
            {
                if (fabs(A(i, j)) > delta)
                {
                    A.Rotate(Q, i, j);
                    check = false;
                }
            }
        }
        // 如果所有分量都小于关值，就减小关值
        if (check)
            delta /= sigma;
        count++;
    }
    return Q;
}

// 二分法，计算三对角对称阵的全部特征值
CAGD::Vector CAGD::LinearSolver::Bisection(const Matrix &A)
{
    assert(A.Row() == A.Col());

    // 特征值范围由无穷范数控制
    int n = A.Row();
    double u = A.Norm();
    double l = -1 * u;

    // 初始化系数
    Vector lambda(n), alpha(n), beta(n);
    for (int i = 0; i < n; i++)
    {
        alpha[i] = A(i, i);
        if (i > 0)
            beta[i] = A(i, i - 1);
    }

    // 计算 n 个特征值
    for (int i = 0; i < n; i++)
    {
        double _u = u;
        double _l = l;
        double nowE = u - l;
        double r = (u + l) / 2;
        while (nowE > ACCURACY)
        {
            // 计算 sn(r)，即 q1(r),q2(r),...,qn(r) 中负数的个数
            int s = 0;
            double q = alpha[0] - r;
            for (int j = 0; j < n; j++)
            {
                if (q < 0)
                    s++;

                if (j < n - 1)
                {
                    if (q == 0)
                        // replace q with a little positive number
                        q = fabs(beta[j + 1]) * ACCURACY;
                    q = alpha[j + 1] - r - beta[j + 1] * beta[j + 1] / q;
                }
            }
            // 与 i+1 进行比较
            (s >= (i + 1)) ? _u = r : _l = r;
            r = (_u + _l) / 2;
            nowE = _u - _l;
        }
        lambda[i] = r;
    }
    return lambda;
}

// 返回分解后的对角阵，正交阵存放在 U,V 中
CAGD::Matrix CAGD::LinearSolver::SVD(const Matrix &A, Matrix *U, Matrix *V)
{
    // 要求行数不小于列数
    assert(A.Row() >= A.Col());

    // 先计算上二对角化
    int p, q, n = A.Col(), count = 0;
    Matrix B = A.Didiagonal(U, V);

    // 限制最多迭代 1000 次
    double threshold = ACCURACY;
    while (count < ITERATION)
    {
        // 将足够小的元素归零
        for (int i = 0; i < n - 1; i++)
        {
            if (fabs(B(i, i + 1)) <= ((fabs(B(i, i)) + fabs(B(i + 1, i + 1))) * threshold))
                B(i, i + 1) = 0;
        }

        // 把足够小的对角元归零
        for (int i = 0; i < n; i++)
        {
            if (fabs(B(i, i)) <= B.Norm() * threshold)
                B(i, i) = 0;
        }

        // 划分成三部分
        q = 0;

        // 检查对角阵的范围
        for (int i = n - 1; i > 0; i--)
        {
            // 检查上次对角元，如果很大，说明对角阵到这里结束
            if (fabs(B(i - 1, i)) > threshold)
                break;

            // 已经到达第二列，说明是对角阵，迭代完成
            if (i - 1 == 0)
                return B;

            q++;
        }

        // 从对角阵头部向上，检查对角元之上的次对角元
        p = 1;
        for (int i = n - q - 1; i > 0; i--)
            if (fabs(B(i - 1, i)) > threshold)
                p++;

        // 得到上面未约化的矩阵阶数
        p = n - q - p;

        // 检查中间不可约部分是否有对角元是零，如果是，那么就将对应行变换归零，然后返回
        // 注意应当倒过来检查，防止出现后面的对角元也是零的情况
        // 不用检查最后一个对角元是零的情况
        bool jump = false;
        for (int i = n - q - 2; i >= p; i--)
        {
            if (B(i, i) == 0)
            {
                // 将 i 行化为零，范围是中间不可约部分
                for (int j = i + 1; j < n - q; j++)
                {
                    // 注意这里 Givens 变换要倒过来算，要将第一个元素化为零
                    Vector x({B(i, j), B(j, j)});
                    Vector cs = Vector::Givens(x);
                    double tmp = cs[0];
                    cs[0] = cs[1];
                    cs[1] = -tmp;

                    // 计算变换
                    B(i, j) = 0;
                    B(j, j) = x.Norm();

                    // 如果不是最后一列，就对下一列进行变换
                    if (j < n - q - 1)
                    {
                        B(i, j + 1) = cs[1] * B(j, j + 1);
                        B(j, j + 1) = cs[0] * B(j, j + 1);
                    }

                    // 累计正交变换
                    if (U != nullptr)
                        U->AG(i, j, cs);
                }

                // 重新二对角化
                B = B.Didiagonal(U, V);
                jump = true;
                break;
            }
        }

        // 对中间的不可约部分进行 Wilkinson SVD 迭代
        if (!jump)
            B.WilkinsonSVD(p, q, U, V);

        count++;
    }
    return B;
}
